/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */

#include "foc_smo.h"
#include "foc_pid.h"
#include "foc_util.h"

/* Constant Macro */
#define M_PI ((float)(3.141592654))
#define M_PI_2 ((float)(M_PI / 2.0))
#define M_PI_4 ((float)(M_PI / 4.0))
#define RL_2PI ((float)(2.0 * M_PI))
#define RL_1D1MS ((float)1000.0)
#define RL_EPSILON_F ((float)0.00000001)
#define RL_A ((float)0.0776509570923569)
#define RL_B ((float)-0.287434475393028)
#define RL_C ((float)(M_PI_4 - RL_A - RL_B))
#define CHEB_SIN_1 ((float)0.999978675)
#define CHEB_SIN_3 ((float)-0.1664971)
#define CHEB_SIN_5 ((float)0.00799224)

#define SPEED_FIFO_COUNT   (41U)

/* Type definitions */
typedef struct {
    /* input information */
    float Ialpha;
    float Ibeta;
    float Ualpha;
    float Ubeta;
    /* ouput estamation */
    float IalphaHat;
    float IbetaHat;
    float EalphaHat;
    float EbetaHat;
    /* intermediate variables */
    float Balpha;  /* Ealpha tilde*/
    float Bbeta;  /* Ebeta tilde*/
    /* ctrl state */
    uint8_t state;/* init force estimate */
    uint16_t cnt;
} SMO_PROC_S;

typedef struct{
    /* input */
    float x;  /* raw signal */
    float Wc;  /* cross frequency */
    /* output */
    float y;  /* filtered signal */
    /* intermediate variables */
    float xLatch;
} DynFilter;

typedef struct{
    float WeHat;  /* estimated electric rotor angular speed */
    float WmHat;  /* estimated mechanical rotor angular speed */
    float WeRaw;  /* un-filtered estimated We */
    float SUMdTH;
    uint16_t cntFIFO;
    /* The max supported loop time is 25us --> 40u for 1ms. */
    float FIFOdTH[SPEED_FIFO_COUNT];  /* FIFO for dTH */
} Speed;

typedef struct{
    float boundaryI;  /* boundary layer of current observer */
    float m;  /* gain of current observer */
    float lambda;  /* desired eigenvalue of BEMF observer */
    float wcSpeedFil;  /* cross frequency of speed filter */
    float pwmFreq;  /* sampling frequency */
    float rs;  /* Resistance in Ohms */
    float ls;  /* Inductance in Henrys */
    float polenum;  /* # of pole pairs */
    float speedRefTime;  /* The time period where BEMF observer uses speed reference. */
    float expRsLsTs;  /* exp(-Rs/Ls*Ts) */
} InputPara;

typedef struct{
    float boundaryI;  /* boundary layer of current observer */
    float m;  /* gain of current observer */
    float lambda;  /* desired eigenvalue of BEMF observer */
    float ls;  /* Inductance in Henrys */
    float speedRefCnt;
    float cnt1ms;
    float mdbi;
    float ts;
    float wcTs;
    float oneMinusWcTs;
    float eleRadPsToMecRPM;
    float aIObs;
    float b1IObs;
    float b2IObs;
    float b3IObs;
} EstimatorPara;

typedef struct{
    float Ialpha;
    float Ibeta;
    float Ualpha;
    float Ubeta;
    float WeRef;  /* electric angular speed reference */
    uint16_t startObs;  /* (flag) start observer: 0u -- no start; 0xFFFFu -- start. */
    tagSpeed speedData;
    tagInputPara para;
} tagObserverInput;

/* global variable definitions */
static tagSMO SMOdata;
static tagEstimatorPara estimatorPara;
static tagStateCtrl estimatorState;

static tagDynFilter dynFilterAlpha1;
static tagDynFilter dynFilterAlpha2;
static tagDynFilter dynFilterBeta1;
static tagDynFilter dynFilterBeta2;
static float WeObs;  /* electric angular speed for BEMF observer */
static float WcFil;  /* cross frequency for BEMF filter */
static float coefX;  /* input coefficient of dynamic filter */
static float coefY;  /* output coefficient of dynamic filter */
/* coefficients of BEMF observer */
static float a1BEMFobs;
static float a2BEMFobs;
static float b1BEMFobs;
static float b2BEMFobs;

tagObserverInput observerInput;

static inline float sinChebyshevF(float x)
{
    /* Chebyshev approximation of sin(x), x is in [-pi/2, pi/2]:
    * 0.999978675*x - 0.1664971*x^3 + 0.00799224*x^5
    */
    float y;
    float tmp1;
    float tmp2;
    
    /* We assume the input x is in [-pi, pi], no guard locates here. */
    /* Map the input angle into [-pi/2, pi/2]. */
    if((float)M_PI_2 < x)
        y = (float)M_PI - x;
    else if((float)(-M_PI_2) > x)
        y = (float)(-M_PI) - x;
    else
        y = x;
    
    tmp1 = y * y;
    tmp2 = y * (CHEB_SIN_1 + tmp1 * (CHEB_SIN_3 + (tmp1 * CHEB_SIN_5)));
    
    return tmp2;
}
static inline float cosChebyshevF(float x)
{
    float y;
    
    /* We assume the input x is in [-pi, pi], no guard locates here. */
    /* Map the input angle into [pi/2, -pi/2]. */    
    if(0.0 > x)
        y = (float)M_PI_2 + x;
    else
        y = (float)M_PI_2 - x;
    
    return sinChebyshevF(y);
}

void initSmo(void)
{
    estimatorState.state = 0u;

    SMOdata.IalphaHat = 0.0f;
    SMOdata.IbetaHat = 0.0f;
    SMOdata.EalphaHat = 0.0f;
    SMOdata.EbetaHat = 0.0f;

    dynFilterAlpha1.xLatch = 0.0f;
    dynFilterAlpha1.y = 0.0f;
    dynFilterAlpha2.xLatch = 0.0f;
    dynFilterAlpha2.y = 0.0f;
    dynFilterBeta1.xLatch = 0.0f;
    dynFilterBeta1.y = 0.0f;
    dynFilterBeta2.xLatch = 0.0f;
    dynFilterBeta2.y = 0.0f;

    observerInput.speedData.cntFIFO = 0u;    
    for(uint16_t tmp=0u; estimatorPara.cnt1ms >= (float)tmp; tmp++)
        observerInput.speedData.FIFOdTH[tmp] = 0.0f;
    
    float tmp1;
    estParaP->boundaryI = inputParaP->boundaryI;
    estParaP->m = inputParaP->m;
    estParaP->lambda = inputParaP->lambda;
    estParaP->ls = inputParaP->ls;
    estParaP->speedRefCnt = (inputParaP->pwmFreq * inputParaP->speedRefTime);
    estParaP->cnt1ms = (inputParaP->pwmFreq * 0.001f);
    estParaP->mdbi = (float)(inputParaP->m / inputParaP->boundaryI);
    estParaP->ts = (float)(1.0f / inputParaP->pwmFreq);
    estParaP->wcTs = (float)(inputParaP->wcSpeedFil * estParaP->ts);
    estParaP->oneMinusWcTs = (float)(1.0f - estParaP->wcTs);
    tmp1 = (float)(1.0f / inputParaP->P);  /* 1 / #_of_pole_pairs */
    estParaP->eleRadPsToMecRPM = (float)(tmp1 * 60.0f / RL_2PI);
    tmp1 = inputParaP->expRsLsTs;
    estParaP->aIObs = tmp1;
    estParaP->b1IObs = (tmp1 - 1.0f) / inputParaP->rs;
    estParaP->b2IObs = -estParaP->b1IObs;
    estParaP->b3IObs = inputParaP->ls * estParaP->b2IObs;  

    estimatorState.cnt1 = 0u;  
}
void execSMO(float ialpha, float ibeta, float ualpha, float ubeta)
{
    /* SMO process
    1.current Observer,estimate Ialpha and Ibeta
    2.BEMF observer
    */

}
void execPLL(void)
{
    /* PLL process
    1.
    */
    
}

void procSmo(float ialpha, float ibeta, float ualpha, float ubeta)
{
    execSMO();
    execPLL();
}
uint16_t getSmoAngel(void)
{

}